import tkinter as tk
from PIL import Image, ImageDraw
import json
import math
import asyncio
import threading
import websockets
import math
from PIL import Image, ImageDraw, ImageFont
class MapViewer:
    def __init__(self, root, file_path, center_utm, scale, canvas_size, base_station_utm):
        self.file_path = file_path
        self.center_utm = center_utm
        self.base_station_utm = base_station_utm
        self.scale = scale
        self.canvas_size = canvas_size
        self.center_pixel = (canvas_size[0] // 2, canvas_size[1] // 2)
        self.offset = [0, 0]  # Offset for panning
        self.zoom_factor = 1.0  # Zoom factor
        self.car_utm = (444560.88, 4426993.81)  # Initial car position (Easting, Northing)
        self.car_yaw = 0  # Initial car yaw angle (in degrees)

        # Initialize tkinter canvas
        self.canvas = tk.Canvas(root, width=canvas_size[0], height=canvas_size[1], bg="white")
        self.canvas.pack(fill=tk.BOTH, expand=True)
        self.canvas.bind("<ButtonPress-3>", self.start_pan)
        self.canvas.bind("<B3-Motion>", self.pan_map)
        self.canvas.bind("<MouseWheel>", self.zoom_map)
        self.canvas.bind("<Button-1>", self.on_click)  # Left-click binding to get UTM coordinates

        # Read and process data
        coords = self.read_coordinates()
        self.relative_coords = self.convert_to_relative(coords)

        # Draw initial map
        self.draw_map()

    def read_coordinates(self):
        """Read coordinates from a text file."""
        with open(self.file_path, 'r') as file:
            lines = file.readlines()
        coordinates = []
        for line in lines:
            if line.strip():
                points = line.strip().split(',')
                coordinates.append([(float(points[i]), float(points[i + 1])) for i in range(0, len(points), 2)])
        return coordinates

    def convert_to_relative(self, coords):
        """Convert UTM coordinates to relative pixel coordinates."""
        relative_coords = []
        for segment in coords:
            relative_segment = []
            for x, y in segment:
                rel_x = (x - self.center_utm[0]) / self.scale
                rel_y = (self.center_utm[1] - y) / self.scale  # Invert Y-axis for tkinter
                relative_segment.append((rel_x, rel_y))
            relative_coords.append(relative_segment)
        return relative_coords

    def draw_map(self):
        """Draw the map on the canvas and add the car model."""
        self.canvas.delete("all")  # Clear the canvas
        # Draw the map's lines (same as before)
        for segment in self.relative_coords:
            for i in range(len(segment) - 1):
                x1, y1 = segment[i]
                x2, y2 = segment[i + 1]
                self.canvas.create_line(
                    self.center_pixel[0] + x1 * self.zoom_factor + self.offset[0],
                    self.center_pixel[1] + y1 * self.zoom_factor + self.offset[1],
                    self.center_pixel[0] + x2 * self.zoom_factor + self.offset[0],
                    self.center_pixel[1] + y2 * self.zoom_factor + self.offset[1],
                    fill="black", width=4
                )

        # Convert the car's UTM coordinates to relative pixel coordinates
        car_rel_x = (self.car_utm[0] - self.center_utm[0]) / self.scale
        car_rel_y = (self.center_utm[1] - self.car_utm[1]) / self.scale  # Invert Y-axis for tkinter

        # Define the size of the car model (rectangle)
        car_length = 42  # Length of the rectangle in pixels
        car_width = 21   # Width of the rectangle in pixels

        # Adjust the corners to rotate the car body by 90 degrees
        corners = [
            (-car_width / 2, -car_length / 2),  # Rear-left
            (car_width / 2, -car_length / 2),   # Rear-right
            (car_width / 2, car_length / 2),    # Front-right (car head)
            (-car_width / 2, car_length / 2)    # Front-left (car head)
        ]

        # Calculate the rotated rectangle points based on yaw
        angle = math.radians(self.car_yaw)  # Convert yaw to radians
        rotated_corners = [
            (
                self.center_pixel[0] + car_rel_x * self.zoom_factor + self.offset[0] + (x * math.cos(angle) - y * math.sin(angle)),
                self.center_pixel[1] + car_rel_y * self.zoom_factor + self.offset[1] + (x * math.sin(angle) + y * math.cos(angle))
            )
            for x, y in corners
        ]

        # Draw the car model
        self.canvas.create_polygon(rotated_corners, fill="red", outline="black", width=2)

        # Convert the center and base station UTM coordinates to relative pixel coordinates
        center_rel_x = (self.center_utm[0] - self.center_utm[0]) / self.scale
        center_rel_y = (self.center_utm[1] - self.center_utm[1]) / self.scale  # Invert Y-axis for tkinter

        base_station_rel_x = (self.base_station_utm[0] - self.center_utm[0]) / self.scale
        base_station_rel_y = (self.center_utm[1] - self.base_station_utm[1]) / self.scale  # Invert Y-axis for tkinter

        # Draw the red circle for the center point (center_utm)
        self.canvas.create_oval(
            self.center_pixel[0] + center_rel_x * self.zoom_factor + self.offset[0] - 5,
            self.center_pixel[1] + center_rel_y * self.zoom_factor + self.offset[1] - 5,
            self.center_pixel[0] + center_rel_x * self.zoom_factor + self.offset[0] + 5,
            self.center_pixel[1] + center_rel_y * self.zoom_factor + self.offset[1] + 5,
            fill="red", outline="red"
        )

        # Draw the red circle for the base station (base_station_utm)
        self.canvas.create_oval(
            self.center_pixel[0] + base_station_rel_x * self.zoom_factor + self.offset[0] - 5,
            self.center_pixel[1] + base_station_rel_y * self.zoom_factor + self.offset[1] - 5,
            self.center_pixel[0] + base_station_rel_x * self.zoom_factor + self.offset[0] + 5,
            self.center_pixel[1] + base_station_rel_y * self.zoom_factor + self.offset[1] + 5,
            fill="red", outline="red"
        )




    def update_car_position(self, utm_x, utm_y, yaw):
        """Update the car's position and redraw the map."""
        self.car_utm = (utm_x, utm_y)
        self.car_yaw = yaw
        self.draw_map()

    def start_pan(self, event):
        """Record the starting position for panning."""
        self.start_x = event.x
        self.start_y = event.y

    def pan_map(self, event):
        """Handle panning of the map."""
        dx = event.x - self.start_x
        dy = event.y - self.start_y
        self.offset[0] += dx
        self.offset[1] += dy
        self.start_x = event.x
        self.start_y = event.y
        self.draw_map()

    def zoom_map(self, event):
        """Handle zooming of the map."""
        factor = 1.1 if event.delta > 0 else 0.9
        self.zoom_factor *= factor
        self.draw_map()

    def on_click(self, event):
        """Handle click on the canvas and convert the pixel to UTM coordinates."""
        # Get the pixel coordinates where the user clicked
        pixel_x = event.x - self.center_pixel[0] - self.offset[0]
        pixel_y = event.y - self.center_pixel[1] - self.offset[1]

        # Convert pixel coordinates back to UTM coordinates
        utm_x = self.center_utm[0] + pixel_x * self.scale * self.zoom_factor
        utm_y = self.center_utm[1] - pixel_y * self.scale * self.zoom_factor  # Invert Y-axis for UTM

        # Print the UTM coordinates
        print(f"UTM Coordinates: ({utm_x:.2f}, {utm_y:.2f})")
    def save_map_as_image(self):
        """Save the map as PNG and PGM files."""
        image = Image.new("RGB", self.canvas_size, "white")
        draw = ImageDraw.Draw(image)
        for segment in self.relative_coords:
            for i in range(len(segment) - 1):
                x1, y1 = segment[i]
                x2, y2 = segment[i + 1]
                draw.line(
                    [
                        (self.center_pixel[0] + x1 * self.zoom_factor + self.offset[0],
                         self.center_pixel[1] + y1 * self.zoom_factor + self.offset[1]),
                        (self.center_pixel[0] + x2 * self.zoom_factor + self.offset[0],
                         self.center_pixel[1] + y2 * self.zoom_factor + self.offset[1])
                    ],
                    fill="black", width=4
                )
        image.save(r"D:\files\农业机器人任务规划研究\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\map_single_v2.png")
        # image.convert("L").save(r"C:\Users\17963\Desktop\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\map.pgm")




    def save_map_as_image_all(self):
        """Save the entire map, including paths, car model, base station, and labels, as a PNG file."""
        
        # 创建空白图像，白色背景
        image = Image.new("RGB", self.canvas_size, "white")
        draw = ImageDraw.Draw(image)

        # 1. 绘制地图路径
        for segment in self.relative_coords:
            for i in range(len(segment) - 1):
                x1, y1 = segment[i]
                x2, y2 = segment[i + 1]
                draw.line(
                    [
                        (self.center_pixel[0] + x1 * self.zoom_factor + self.offset[0],
                        self.center_pixel[1] + y1 * self.zoom_factor + self.offset[1]),
                        (self.center_pixel[0] + x2 * self.zoom_factor + self.offset[0],
                        self.center_pixel[1] + y2 * self.zoom_factor + self.offset[1])
                    ],
                    fill="black", width=4
                )

        # 2. 计算并绘制车辆模型（红色矩形）
        car_rel_x = (self.car_utm[0] - self.center_utm[0]) / self.scale
        car_rel_y = (self.center_utm[1] - self.car_utm[1]) / self.scale  # Y 轴翻转

        car_length = 42  # 车辆长度（像素）
        car_width = 21   # 车辆宽度（像素）

        # 计算旋转矩形角点
        angle = math.radians(self.car_yaw)
        corners = [
            (-car_width / 2, -car_length / 2),  # 后左
            (car_width / 2, -car_length / 2),   # 后右
            (car_width / 2, car_length / 2),    # 前右
            (-car_width / 2, car_length / 2)    # 前左
        ]
        rotated_corners = [
            (
                self.center_pixel[0] + car_rel_x * self.zoom_factor + self.offset[0] + (x * math.cos(angle) - y * math.sin(angle)),
                self.center_pixel[1] + car_rel_y * self.zoom_factor + self.offset[1] + (x * math.sin(angle) + y * math.cos(angle))
            )
            for x, y in corners
        ]

        draw.polygon(rotated_corners, fill="red", outline="black")

        # 3. 绘制中心点和基站（红色圆点）
        def draw_circle(draw, x, y, radius, color):
            draw.ellipse(
                [x - radius, y - radius, x + radius, y + radius],
                fill=color, outline=color
            )

        center_rel_x = (self.center_utm[0] - self.center_utm[0]) / self.scale
        center_rel_y = (self.center_utm[1] - self.center_utm[1]) / self.scale

        base_station_rel_x = (self.base_station_utm[0] - self.center_utm[0]) / self.scale
        base_station_rel_y = (self.center_utm[1] - self.base_station_utm[1]) / self.scale

        center_x = self.center_pixel[0] + center_rel_x * self.zoom_factor + self.offset[0]
        center_y = self.center_pixel[1] + center_rel_y * self.zoom_factor + self.offset[1]
        
        base_x = self.center_pixel[0] + base_station_rel_x * self.zoom_factor + self.offset[0]
        base_y = self.center_pixel[1] + base_station_rel_y * self.zoom_factor + self.offset[1]
        
        goal_0_x_rel = (444562 - self.center_utm[0]) / self.scale
        goal_0_y_rel = (4426999 - self.center_utm[1]) / self.scale

        goal_0_x = self.center_pixel[0] + goal_0_x_rel * self.zoom_factor + self.offset[0]
        goal_0_y = self.center_pixel[1] + goal_0_y_rel * self.zoom_factor + self.offset[1]

        goal_1_x_rel = (444566.6 - self.center_utm[0]) / self.scale
        goal_1_y_rel = (4426992.5 - self.center_utm[1]) / self.scale

        goal_1_x = self.center_pixel[0] + goal_1_x_rel * self.zoom_factor + self.offset[0]
        goal_1_y = self.center_pixel[1] + goal_1_y_rel * self.zoom_factor + self.offset[1]
        draw_circle(draw, center_x, center_y, 5, "red")
        draw_circle(draw, base_x, base_y, 5, "red")
        draw_circle(draw, goal_0_x, goal_0_y, 5, "red")
        draw_circle(draw, goal_1_x, goal_1_y, 5, "red")
        # 4. 添加文字标签
        try:
            font = ImageFont.truetype("arial.ttf", 18)  # 使用 Arial 字体
        except:
            font = ImageFont.load_default()  # 如果 Arial 字体不可用，使用默认字体

        text_offset = 10  # 文字与点的垂直间距

        draw.text((center_x - 20, center_y + text_offset), f"Center({center_rel_x},{center_rel_y})", fill="black", font=font)
        draw.text((base_x - 20, base_y + text_offset), f"Base_Station({base_station_rel_x:.2f},{base_station_rel_y:.2f})", fill="black", font=font)
        draw.text((goal_0_x - 20, goal_0_y + text_offset), f"goal_0({goal_0_x_rel:.2f},{goal_0_y_rel:.2f})", fill="black", font=font)
        draw.text((goal_1_x - 20, goal_1_y + text_offset), f"goal_1({goal_1_x_rel:.2f},{goal_1_y_rel:.2f})", fill="black", font=font)
        
        # draw.rectangle([100, 100, 200, 200], fill="blue", outline="black")  # 100,100 到 200,200 之间填充蓝色

        # 5. 保存图片
        file_path = r"D:\files\农业机器人任务规划研究\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\map_single_v2_all.png"
        image.save(file_path)
        print(f"地图已保存到: {file_path}")


    def save_map_metadata(self):
        """保存地图的元数据，包括基准站坐标和偏移量"""
        offset_x = self.center_utm[0] - self.base_station_utm[0]  # 东向（右）
        offset_y = self.center_utm[1] - self.base_station_utm[1]  # 北向（上）

        metadata = {
            "map_size": self.canvas_size,
            "scale": self.scale,
            "center_utm": self.center_utm,
            "base_station_utm": self.base_station_utm,
            "offset_from_base_station": [offset_x, offset_y]  # 相对偏移量
        }

        # 这里假设你有保存到文件的逻辑，比如写入 JSON 文件等

        with open(r"D:\files\农业机器人任务规划研究\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\map_single_v2.json", "w") as json_file:
            json.dump(metadata, json_file, indent=4)


async def websocket_listener(viewer, ws_url):
    """Asynchronous WebSocket listener to update the car's position in real-time."""
    async with websockets.connect(ws_url) as websocket:
        while True:
            message = await websocket.recv()
            if message.startswith("utm|"):
                _, easting, northing, yaw = message.split("|")
                viewer.update_car_position(float(easting), float(northing), float(yaw))


def start_websocket_listener(viewer, ws_url):
    """Start the asyncio event loop for the WebSocket listener."""
    loop = asyncio.new_event_loop()
    asyncio.set_event_loop(loop)
    loop.run_until_complete(websocket_listener(viewer, ws_url))

import math

def calculate_canvas_size(file_path, center_utm, scale, margin_pixels=10):
    """动态计算画布大小，确保地图完全显示并从中心向外扩展"""
    with open(file_path, 'r') as file:
        lines = file.readlines()

    # 找到所有的 UTM 坐标的最小和最大值
    utm_coords = []
    for line in lines:
        if line.strip():
            points = line.strip().split(',')
            utm_coords.extend([(float(points[i]), float(points[i + 1])) for i in range(0, len(points), 2)])

    # 获取 X 和 Y 的最小值和最大值
    min_x = min(coord[0] for coord in utm_coords)
    max_x = max(coord[0] for coord in utm_coords)
    min_y = min(coord[1] for coord in utm_coords)
    max_y = max(coord[1] for coord in utm_coords)

    # 计算中心点到地图边缘的最大距离
    max_dist_x = max(abs(center_utm[0] - min_x), abs(center_utm[0] - max_x))
    max_dist_y = max(abs(center_utm[1] - min_y), abs(center_utm[1] - max_y))

    # 动态计算画布的像素大小，以确保地图完全显示
    width = (max_dist_x * 2) / scale
    height = (max_dist_y * 2) / scale
    # 加上边距
    width += 2 * margin_pixels
    height += 2 * margin_pixels
    # 向上取整到百的倍数
    width = math.ceil(width / 100) * 100
    height = math.ceil(height / 100) * 100



    # 返回画布大小
    return width, height


def calculate_center_utm(file_path):
    """动态计算地图中心点"""
    with open(file_path, 'r') as file:
        lines = file.readlines()

    # 找到所有的 UTM 坐标的最小和最大值
    utm_coords = []
    for line in lines:
        if line.strip():
            points = line.strip().split(',')
            utm_coords.extend([(float(points[i]), float(points[i + 1])) for i in range(0, len(points), 2)])

    # 获取 X 和 Y 的最小值和最大值
    min_x = min(coord[0] for coord in utm_coords)
    max_x = max(coord[0] for coord in utm_coords)
    min_y = min(coord[1] for coord in utm_coords)
    max_y = max(coord[1] for coord in utm_coords)

    # 计算中心点坐标
    center_x = (min_x + max_x) / 2
    center_y = (min_y + max_y) / 2

    # 返回中心点坐标
    return center_x, center_y


def main():
    # Parameters
    file_path = r"D:\files\农业机器人任务规划研究\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\utm_coordinates.txt"  # Replace with your file name
    # center_utm = (444431.969868, 4427046.542976)
    center_utm = calculate_center_utm(file_path)  # 动态计算地图中心点
    scale = 0.05  # 1 pixel = 0.05 meters
    # canvas_size = (5000, 5000)  # Canvas size in pixels
    base_station_utm = (444557.5094656385,4426995.800712771)
        # 计算动态画布大小
    canvas_size = calculate_canvas_size(file_path, center_utm, scale)
    ws_url = "ws://39.106.77.250:8089/ws?command_receiver&utm"  # Replace with your WebSocket server URL

    # Create Tkinter window
    root = tk.Tk()
    root.title("UTM Map Viewer")
    print(canvas_size)
    # Create MapViewer instance
    viewer = MapViewer(root, file_path, center_utm, scale, canvas_size, base_station_utm)

    viewer.save_map_as_image()
    viewer.save_map_metadata()
    viewer.save_map_as_image_all()
    # Start WebSocket listener in a separate thread
    threading.Thread(target=start_websocket_listener, args=(viewer, ws_url), daemon=True).start()

    # Run Tkinter loop
    root.mainloop()


if __name__ == "__main__":
    main()
